/*
* Copyright 2018 Jens Willy Johannsen <jens@jwrobotics.com>, JW Robotics
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* SBUS driver
* Based on code from:
*	https://github.com/uzh-rpg/rpg_quadrotor_control
*/

#pragma once

#include <atomic>
#include <thread>
#include <functional>
#include <array>

#include <asm/ioctls.h>
#include <asm/termbits.h>
#include <deque>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <sys/ioctl.h>
#include <cstdio>
#include <unistd.h>

#define RC_CHANNEL_MIN 990
#define RC_CHANNEL_MAX 2010
#define SBUS_MIN_OFFSET 173
#define SBUS_MID_OFFSET 992
#define SBUS_MAX_OFFSET 1811

int serial_port_fd_;

uint8_t radioMsg[25] = {0x00};
int channels[16] = {1500};

bool configureSerialPortForSBus()
{
    // clear config
    fcntl( serial_port_fd_, F_SETFL, 0 );
    // read non blocking
    fcntl( serial_port_fd_, F_SETFL, FNDELAY );

    struct termios2 uart_config;
    /* Fill the struct for the new configuration */
    ioctl( serial_port_fd_, TCGETS2, &uart_config );

    // Output flags - Turn off output processing
    // no CR to NL translation, no NL to CR-NL translation,
    // no NL to CR translation, no column 0 CR suppression,
    // no Ctrl-D suppression, no fill characters, no case mapping,
    // no local output processing
    //
    uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);

    // Input flags - Turn off input processing
    // convert break to null byte, no CR to NL translation,
    // no NL to CR translation, don't mark parity errors or breaks
    // no input parity check, don't strip high bit off,
    // no XON/XOFF software flow control
    //
    uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK
                    | ISTRIP | IXON);

    //
    // No line processing:
    // echo off
    // echo newline off
    // canonical mode off,
    // extended input processing off
    // signal chars off
    //
    uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);

    // Turn off character processing
    // Turn off odd parity
    uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD);

    // Enable parity generation on output and parity checking for input.
    uart_config.c_cflag |= PARENB;
    // Set two stop bits, rather than one.
    uart_config.c_cflag |= CSTOPB;
    // No output processing, force 8 bit input
    uart_config.c_cflag |= CS8;
    // Enable a non standard baud rate
    uart_config.c_cflag |= BOTHER;

    // Set custom baud rate of 100'000 bits/s necessary for sbus
    const speed_t spd = 100000;
    uart_config.c_ispeed = spd;
    uart_config.c_ospeed = spd;

    if( ioctl( serial_port_fd_, TCSETS2, &uart_config ) < 0 )
    {
        printf( "[Could not set configuration of serial port\n" );
        return false;
    }

    return true;
}

bool connectSerialPort( const std::string& port )
{
    // Open serial port
    // O_RDWR - Read and write
    // O_NOCTTY - Ignore special chars like CTRL-C
    serial_port_fd_ = open( port.c_str(), O_RDWR | O_NOCTTY );
    printf( "Connect to serial port\n" );

    if( serial_port_fd_ == -1 )
    {
        printf( "Could not open serial port: '%s'\n", port.c_str() );
        return false;
    }

    if( !configureSerialPortForSBus())
    {
        ::close( serial_port_fd_ );
        printf( "Could not set necessary configuration of serial port\n" );
        return false;
    }

    return true;
}

bool setUpSBusSerialPort( const std::string& port)
{
    if( !connectSerialPort( port ))
    {
        return false;
    }

    return true;
}

void sbusPreparePacket(uint8_t packet[], int channels[])
{
    uint16_t output[16] = {0};//这里一定要16个元素的数组，不然其他通道会干扰
  /*将chanel值1000-2000映射到SBUS协议的173-1811*/
  for (uint8_t i = 0; i < 16; i++) {

    //   output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
      output[i] = (channels[i] - RC_CHANNEL_MIN) * (SBUS_MAX_OFFSET - SBUS_MIN_OFFSET) / (RC_CHANNEL_MAX - RC_CHANNEL_MIN) + SBUS_MIN_OFFSET;
  }

  packet[0] = 0x0f; //SBUS头，0x0F
  packet[1] = (uint8_t) (output[0] & 0x07FF);
  packet[2] = (uint8_t) ((output[0] & 0x07FF) >> 8 | (output[1] & 0x07FF) << 3);
  packet[3] = (uint8_t) ((output[1] & 0x07FF) >> 5 | (output[2] & 0x07FF) << 6);
  packet[4] = (uint8_t) ((output[2] & 0x07FF) >> 2);
  packet[5] = (uint8_t) ((output[2] & 0x07FF) >> 10 | (output[3] & 0x07FF) << 1);
  packet[6] = (uint8_t) ((output[3] & 0x07FF) >> 7 | (output[4] & 0x07FF) << 4);
  packet[7] = (uint8_t) ((output[4] & 0x07FF) >> 4 | (output[5] & 0x07FF) << 7);
  packet[8] = (uint8_t) ((output[5] & 0x07FF) >> 1);
  packet[9] = (uint8_t) ((output[5] & 0x07FF) >> 9 | (output[6] & 0x07FF) << 2);
  packet[10] = (uint8_t) ((output[6] & 0x07FF) >> 6 | (output[7] & 0x07FF) << 5);
  packet[11] = (uint8_t) ((output[7] & 0x07FF) >> 3);
  packet[12] = (uint8_t) ((output[8] & 0x07FF));
  packet[13] = (uint8_t) ((output[8] & 0x07FF) >> 8 | (output[9] & 0x07FF) << 3);
  packet[14] = (uint8_t) ((output[9] & 0x07FF) >> 5 | (output[10] & 0x07FF) << 6);
  packet[15] = (uint8_t) ((output[10] & 0x07FF) >> 2);
  packet[16] = (uint8_t) ((output[10] & 0x07FF) >> 10 | (output[11] & 0x07FF) << 1);
  packet[17] = (uint8_t) ((output[11] & 0x07FF) >> 7 | (output[12] & 0x07FF) << 4);
  packet[18] = (uint8_t) ((output[12] & 0x07FF) >> 4 | (output[13] & 0x07FF) << 7);
  packet[19] = (uint8_t) ((output[13] & 0x07FF) >> 1);
  packet[20] = (uint8_t) ((output[13] & 0x07FF) >> 9 | (output[14] & 0x07FF) << 2);
  packet[21] = (uint8_t) ((output[14] & 0x07FF) >> 6 | (output[15] & 0x07FF) << 5);
  packet[22] = (uint8_t) ((output[15] & 0x07FF) >> 3);
  packet[23] = 0x00;         // 标志位
  packet[24] = 0x00; // SBUS结束字节
}












